/*
 *  Copyright (C) 2022  贺龙宇
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program. If not, see <http://www.gnu.org/licenses/>.
 */
#include "SolvePos.h"


/*
目前的决策方案：
优先级>同ID>离图像正中心较近
具体操作：
先将同优先级的装甲板放入同一个容器中，
再对ID进行筛选，ID与上一次决策相同的装甲板优先被选择
若此时依然有大于1个装甲板，则选择离图像中心更近的那个
*/


SolvePos::SolvePos(){
    topping_id=0;
    frame_count=0;
    topping_init=false;
#ifdef MANIFOLD_G
    min_frame=5;
    max_frame=30;
    hold_gain=0.9;
    given_frame=30;
#endif
#ifdef MANIFOLD_C
    min_frame=15;
    max_frame=100;
    hold_gain=0.95;
    given_frame=100;
#endif
#ifdef NUC
    min_frame=8;
    max_frame=40;
    hold_gain=0.9;
    given_frame=40;
#endif
    kalman.Init();
}

SolvePos::~SolvePos(){}

cv::Point3f SolvePos::GetTarget(const cv::Mat &src,const std::vector<cv::Rect>& box, std::vector<int> &id)
{
    //Selection
#ifdef CNN
    ArmorFilter(id);
    // std::cout<<id[0]<<std::endl;
#endif

#ifdef DEBUG
    show_final_armor=src.clone();
#endif
    // double t=std::clock();
    frame_count++;
    /*如果是英雄，且状态不是自瞄，则认为是进行前哨站预测。*/
    /*RUNE_SMALL为向左运动，RUNE_BIG为向右运动*/
    /*在比赛中，暂时弃用这一方案*/
    if(false){
        SelectOutpost(box,id,this->selected_rect);
        
        if(this->selected_rect.width==0 && this->selected_rect.height==0)
        {
            return {0,0,-1};
        }
        cv::Rect predict_rect;
        predict_rect=OutpostPredict(selected_rect);

        cv::Point3f target=SolveControlInfo(predict_rect);/*解算三个数值*/
        //cv::Point3f target = SolveTarget(src,selected_rect,selected_rect_id); //NEW METHOD

#ifdef CNN
        //float distance_T = CalcDistance(src,selected_rect);
    
        //if(distance_T>0) target.z=distance_T;   
#endif
        this->DistanceFilter(target,selected_rect_id);/*对距离进行滤波处理，缓解测距波动*/
    
        return target;
    }//OutPost




    else
    {
    /*如果是哨兵的话，则使用逻辑筛选;否则筛选离图像中心近的目标*/
    if(mcu_data.id==7)
        SelectArmor_AUTO(box,id,this->selected_rect);/*筛选装甲板*/
    else
        SelectArmor_MANUAL(box,id,this->selected_rect);
    if(this->selected_rect.width==0 && this->selected_rect.height==0)
    {
        return {0,0,-1};
    }

    /*如果是哨兵的话，当识别到基地和前哨站则不攻击*/
    if(mcu_data.id==7 && (selected_rect_id==6 || selected_rect_id==8)) {
        return {0,0,-1};
    }

    /*如果是哨兵的话，当识别到工程则不攻击*/
    // if(mcu_data.id==7 && (selected_rect_id==2)) {
    //     return {0,0,-1};
    // }

#ifdef DEBUG
    show_final_armor=src.clone();
    cv::rectangle(show_final_armor,selected_rect,cv::Scalar(0,255,0),2,16);
    cv::imshow("final_rect",show_final_armor);
#endif

    /*反陀螺*/
    ToppingAnalyzer(id);
    // static State last_state=State::ARMOR_STATE; /*保存上一时刻的状态*/

    /*如果上一时刻不是反陀螺状态，则设置topping_reinit为true，即需进行初始化*/
    

    /*如果topping_init变量为true，说明需要进行初始化*/
    if(topping_init){
        TopInit(this->selected_rect);
        topping_init=false;
    }

    /*另外，如果反陀螺状态开，则进行反陀螺识别（能进到这个条件判断说明初始化已经完成）*/
    else if(topping_id)
        this->selected_rect=AntiTop(this->selected_rect);

    /*反陀螺部分结束*/
    }

#ifdef DEBUG
    cv::Mat show_topping=src.clone();
    cv::circle(show_topping,cv::Point(selected_rect.x+0.5*selected_rect.width,selected_rect.y+0.5*selected_rect.height)
                    ,4,cv::Scalar(0,255,0),5,16);
    cv::imshow("topping",show_topping);
#endif

    cv::Point3f target=SolveControlInfo(this->selected_rect);/*解算三个数值*/
//    cv::Point3f target = SolveTarget(src,selected_rect,selected_rect_id); //NEW METHOD

#ifdef CNN
//    float distance_T = CalcDistance(src,selected_rect);
    
//    if(distance_T>0) target.z=distance_T; 

    // if(distance_T>0) 
#endif
    
    this->DistanceFilter(target,selected_rect_id);/*对距离进行滤波处理，缓解测距波动*/
    
    
    

    /*预测部分交由电控完成*/
    // if(this->selected_rect_id==7 && serial_states==SERIAL_NORMAL){
        // target=Predict(target); /*预测*/
    // }

    // printf("(%f %f %f) \n",target.x,target.y,target.z);



    //测试：坐标系转换为旋转轴为原点,对英雄使用
    // if(mcu_data.id==1) 
        // target=Target2GimbalAxis(target);/*坐标轴变换，目前只针对英雄使用*/
    
    //AmmoEstimation 弹道补偿交由电控完成
    // if(abs(target.x)<2.0){
        // if(mcu_data.id!=7) target=AmmoEstimation(target); /*哨兵查表 待优化,暂不对哨兵使用*/
    // }
    
    // printf("time: %lf ms\n",(double)(std::clock()-t)/1000);

    return target;
}

void SolvePos::ArmorFilter(std::vector<int> &id)
{
    
    for(auto i=0;i<id.size();i++)
    {
        
        if(hand_data.enemy_color==EnemyColor::RED && (id[i]<8 || id[i]>15))
        {
            id[i] = 0;
        }
        else if(hand_data.enemy_color==EnemyColor::BLUE && id[i]<17)
        {
            id[i] = 0;
        }
        else
        {
            //id change to 1-8;
            id[i]=(id[i]%9+2)%9;
            // if(id[i]==5) id[i]=3;/*防止3识别成5的情况*/
        }
        
    }
}

void SolvePos::SelectArmor_AUTO(const std::vector<cv::Rect>& box, const std::vector<int> &id, cv::Rect &selected_rect)
{
    static int last_id=-1;
    std::vector<cv::Rect> box_same_priority;
    std::vector<int> id_same_priority;
    int max_id, maxvalue=-1;
    /*Step1: Select out the same priority target*/
    for(auto i=0;i<id.size();i++)
    {
        if(weights[id[i]]>maxvalue)
        {
            max_id=id[i];
            maxvalue=weights[id[i]];
            box_same_priority.clear();
            id_same_priority.clear();
            box_same_priority.emplace_back(box[i]);
            id_same_priority.emplace_back(id[i]);
        }
        else if(weights[id[i]]==maxvalue){
            box_same_priority.emplace_back(box[i]);
            id_same_priority.emplace_back(id[i]);
        }
    }
    /*Max priority is 0 no suitable targets, return false*/ 
    if(maxvalue==0)
    {
        cv::Rect none;
        none.width=0;
        none.height=0;
        selected_rect = none;
        return ;
    }

    /*Step2: Select the ID as same as last frame*/
    std::vector<cv::Rect> same_id_box;
    std::vector<int> same_id_id;
    std::vector<cv::Rect> diff_id_box;
    std::vector<int> diff_id_id;
    for(auto i=0;i<id_same_priority.size();i++){
        if(last_id==id_same_priority[i]){
            same_id_box.emplace_back(box_same_priority[i]);
            same_id_id.emplace_back(id_same_priority[i]);
        }

        else{
            diff_id_box.emplace_back(box_same_priority[i]);
            diff_id_id.emplace_back(id_same_priority[i]);
        }
    }

    /*Step3: Select the target's center near to the picture center*/
    int maxindex;
    if(same_id_id.size()!=0){
        double min_distance=100000007;
        for(auto i=0;i<same_id_id.size();i++){
            double distance=
               ((same_id_box[i].x+0.5*same_id_box[i].width)-0.5*cameraParam.resolution_width)*
               ((same_id_box[i].x+0.5*same_id_box[i].width)-0.5*cameraParam.resolution_width)+
               ((same_id_box[i].y+0.5*same_id_box[i].height)-0.5*cameraParam.resolution_height)*
               ((same_id_box[i].y+0.5*same_id_box[i].height)-0.5*cameraParam.resolution_height);
            if(distance<min_distance){
                min_distance=distance;
                maxindex=i;
            }
        }
        last_id=same_id_id[maxindex];
        selected_rect=same_id_box[maxindex];
    }
    else{
        double min_distance=100000007;
        for(auto i=0;i<diff_id_id.size();i++){
            double distance=
               ((diff_id_box[i].x+0.5*diff_id_box[i].width)-0.5*cameraParam.resolution_width)*
               ((diff_id_box[i].x+0.5*diff_id_box[i].width)-0.5*cameraParam.resolution_width)+
               ((diff_id_box[i].y+0.5*diff_id_box[i].height)-0.5*cameraParam.resolution_height)*
               ((diff_id_box[i].y+0.5*diff_id_box[i].height)-0.5*cameraParam.resolution_height);
            if(distance<min_distance){
                min_distance=distance;
                maxindex=i;
            }
        }
        last_id=diff_id_id[maxindex];
        this->selected_rect=diff_id_box[maxindex];
        this->selected_rect_id=diff_id_id[maxindex];
    }
    
    
}

void SolvePos::SelectArmor_MANUAL(const std::vector<cv::Rect>& box, const std::vector<int> &id, cv::Rect &selected_rect)
{   
    int min_dis=1000;
    int min_dis_index;
    for(int i=0;i<box.size();i++){
        if(id[i]==0) continue;
        else{
            int dis=std::abs(box[i].x+0.5*box[i].width-0.5*cameraParam.resolution_width);
            if(dis<min_dis){
                min_dis=dis;
                min_dis_index=i;
            }
        }
    }
    selected_rect=box[min_dis_index];
}

cv::Point3f SolvePos::SolveControlInfo(const cv::Rect& armor)
{
    static float half_camera_width = cameraParam.resolution_width / 2;
    static float half_camera_height = cameraParam.resolution_height / 2;
    static float camera_fx = cameraParam.fx;
    static float camera_fy = cameraParam.fy;
    

    
    cv::Point2f armor_center = cv::Point2f(armor.x+armor.width/2.0,armor.y+armor.height/2.0);
    float yaw_offset = atan2(half_camera_width - armor_center.x, camera_fx) * rad;
    float pitch_offset = atan2(half_camera_height - armor_center.y, camera_fy) * rad;
    float distance=camera_fx * armorParam.armor_height / armor.height / 1000;
    return cv::Point3f(yaw_offset,pitch_offset,distance);

}

void SolvePos::DistanceFilter(cv::Point3f &target,const int &id){
    
    static int former_id=0; //用于记录前一时刻的id
    static double last_distance=0;
    //如果两者id不同，则不进行滤波处理并更新former_id为当前id,重置last_distance
    if(id!=former_id){
        former_id=id;
        last_distance=target.z;
        return;
    }

    //如果相同，则需要进行滤波处理。对于当前测得的距离，采取25%的置信度（可修改）
    double believe=0.25;
    
    target.z=believe*target.z+(1-believe)*last_distance;
    last_distance=target.z;
}

cv::Point3f SolvePos::AmmoEstimation(const cv::Point3f &target)
{
    //Get distance via TFLuna
    // float distance=GetDistance();
    //TODO: Calculate the model of sentry


    /*Infantry&Hero&default: In the same height*/
    float corrected_pitch;
    
    /*哨兵补偿，具体见文档*/
    if(mcu_data.id>=7){
        double l=std::sqrt(target.z*target.z-orbit_height*orbit_height);
        corrected_pitch=atan(g*l/(std::sqrt(30*30*30*30-g*g*l*l+2*g*orbit_height*30*30)-30*30));
    }

    else{
        corrected_pitch=target.y+
        armorParam.ammo_const_for_ground*(0.5*rad*asin(g*target.z/mcu_data.shoot_speed/mcu_data.shoot_speed));
        //formula: theta=0.5*arcsin(gx/v^2), where x is the distance of the target
    }
    return {target.x,corrected_pitch,target.z};
}

cv::Point3f SolvePos::Predict(const cv::Point3f& src_target){
    //TODO: pos tansfer, so do to pitch
    //Step1: transfer to gimbal axis
    static float last_gimbal_target_yaw=0;
    float this_gimbal_yaw=gimbal_data.yaw*360.0/8192.0;
    float this_gimbal_target_yaw=src_target.x+this_gimbal_yaw;

    //Step2: Predict
    float next_gimbal_target_yaw=armorParam.prediction_correct_const*
                                    (this_gimbal_target_yaw-last_gimbal_target_yaw);

    last_gimbal_target_yaw=this_gimbal_target_yaw;

    //Step3: Transfer back
    float next_target_yaw=next_gimbal_target_yaw-this_gimbal_yaw;

    //Step4: Filter
    static float last_target_yaw=0.0;
    static const float retain_gain=0.85;

    if(std::abs(last_target_yaw)<1e-6){
        last_target_yaw=next_target_yaw;
    }
    else{
        next_target_yaw=(1-retain_gain)*next_target_yaw+retain_gain*last_target_yaw;
        last_target_yaw=next_target_yaw;
    }

    return {next_target_yaw,src_target.y,src_target.z};
}


void SolvePos::ToppingAnalyzer(const std::vector<int> &id){
    int count[9]; /*用于统计各数字出现的次数*/
    memset(count,0,sizeof(count));
    static int topping_frame=0;

    /*枚举容器中的所有数字*/
    for(int i=0;i<id.size();i++){
        if(id[i]==0) continue;
        
        count[id[i]]++;
        
    }
    
    /*若当前不是小陀螺状态，则判断是否满足进入小陀螺状态的条件*/
    if(topping_id==0){
        
        /*记录出现两次（或以上？）的数字*/
        
        static std::queue<int> count_above_twice_in_which_frame[9];
        
        for(int i=1;i<=8;i++){

            if(count[i]>=2) count_above_twice_in_which_frame[i].emplace(frame_count);
            
            if(count_above_twice_in_which_frame[i].size()==2){
              
                /*两次间隔满足条件(在10-50帧之间)，则认为目标正在小陀螺，启动小陀螺模式，并将topping_id设为目标id
                  初始化识别帧数为50帧，并设置小陀螺识别初始化信号,清空对应队列，提高该id在同权重id中的识别优先级*/
                if(count_above_twice_in_which_frame[i].back()-count_above_twice_in_which_frame[i].front()>min_frame
                && count_above_twice_in_which_frame[i].back()-count_above_twice_in_which_frame[i].front()<max_frame){
              
                    topping_id=i;
                    topping_init=true;
                    // std::cout<<"Init\n";
                    count_above_twice_in_which_frame[i].pop();
                    count_above_twice_in_which_frame[i].pop();
                    topping_frame=given_frame;
                    weights[topping_id]++;
                    break;
              
                }
                /*否则队首元素出队*/
                else{
                    count_above_twice_in_which_frame[i].pop();
                }
            }
        }
    }
    /*若当前是小陀螺状态，则判断是否满足退出条件
      首先先让topping_frame--*/
    else{
        topping_frame--;
        //  printf("Topping_frame: %d\n",topping_frame);
        /*若此时依然满足进入反陀螺的逻辑，则说明还在小陀螺，重置topping_frame为50*/

        static std::queue<int> check_queque;

        if(count[topping_id]>=2) {
            check_queque.emplace(frame_count);
            if(check_queque.size()==2){
                if(check_queque.back()-check_queque.front()>min_frame && check_queque.back()-check_queque.front()<max_frame){
                    topping_frame=given_frame;
                }
                check_queque.pop();
            }
            
        }
        /*如果topping_frame减至0，则退出小陀螺模式,并恢复该id的优先级*/
        else if(topping_frame<=0){
                
                while(!check_queque.empty()) check_queque.pop();
                weights[topping_id]--;
                topping_id=0;
            }
    }
}

void SolvePos::TopInit(const cv::Rect &armor){
    /*初始化，将上一时刻的装甲板中心设置为这一时刻的实际值*/
    // this->last_rect_center=cv::Point(armor.x+0.5*armor.width,armor.y+0.5*armor.height);

    /*采用卡尔曼滤波方法，初始化卡尔曼滤波器*/
    kf.Init(armor.x+0.5*armor.width);
}

cv::Rect SolvePos::AntiTop(const cv::Rect& armor){
    cv::Rect target_armor=armor;

    cv::Point armor_center=cv::Point(armor.x+0.5*armor.width,armor.y+0.5*armor.height);
    
    //对装甲板中心点的x方向上做滤波
    
    // double hold_gain=0.95;
    
    /*如果当前位置与上一帧的x方向相差了120px以上，则不使用滤波，直接返回当前位置*/
    /*换句话说，当两帧相差在120px内，则使用滤波进行平滑处理*/

    // std::cout<<last_rect_center.x-armor_center.x<<std::endl;
    // if(std::abs(last_rect_center.x-armor_center.x)<120.0){
    //     armor_center.x=hold_gain*last_rect_center.x+(1-hold_gain)*armor_center.x;
    // }
    // last_rect_center=armor_center;

    /*进行卡尔曼滤波*/
    armor_center.x=kf.Filter(armor_center.x);

    /*将滤波后的中心坐标转换为目标装甲板的中心坐标*/
    target_armor.x=armor_center.x-0.5*target_armor.width;
    target_armor.y=armor_center.y-0.5*target_armor.height;

    return target_armor;
}

void SolvePos::TargetLost(void){
    // topping_init=true;
    //topping_id=0;
    frame_count=0;
}



cv::Point3f SolvePos::Target2GimbalAxis(const cv::Point3f &src_target){
    static const cv::Mat transfer_matrix = //英雄的相机CMOS到云台的距离为0.17m
        (cv::Mat_<float>(4, 4) << 1,0,0,0,
                                  0,1,0,0,
                                  0,0,1,0.17,
                                  0,0,0,1);

    //Step1: 转换为弧度制
    float r=src_target.z;
    float phi=src_target.x/rad;   //偏航
    float theta=src_target.y/rad;//俯仰

    //Step2: 转换为直角坐标系
    float x=r*std::sin(theta)*std::cos(phi);
    float y=r*std::sin(theta)*std::sin(phi);
    float z=r*std::cos(theta);

    //Step3: 乘上转换矩阵
    cv::Mat target_matrix=(cv::Mat_<float>(4,1) << x,y,z,1);

    target_matrix=transfer_matrix*target_matrix;
    
    x=target_matrix.at<float>(0,0);
    y=target_matrix.at<float>(1,0);
    z=target_matrix.at<float>(2,0);
    // printf("%f %f %f\n",x,y,z);
    //Step4：逆变换回极坐标系
    r=std::sqrt(x*x+y*y+z*z);
    theta=std::acos(z/r);
    if(x<0) theta=-theta;
    phi=std::atan(y/x);

    return {phi*rad,theta*rad,r};
    
}

void SolvePos::SelectOutpost(const std::vector<cv::Rect>& box, const std::vector<int> &id, cv::Rect &selected_rect){
    
    int selected_index=-1; /*被选择到的装甲板id*/
    int selected_x_axis=-1;/*选择到的装甲板的x坐标*/
    
    for(int i=0;i<box.size();i++){

        if(id[i]!=6) continue;
        // if(false) continue;
        
        /*如果选择到了前哨站，则判断位置*/
        /*左旋则选择靠右的，右旋则选择靠左的*/
        else{
            
            int this_x_axis=box[i].x+0.5*box[i].width;
            
            if(selected_x_axis==-1){
                selected_index=i;
                selected_x_axis=this_x_axis;
            }
            else if(mcu_data.state==State::RUNE_SMALL && this_x_axis>selected_x_axis)/*左旋,选右*/
            {
                selected_index=i;
                selected_x_axis=this_x_axis;
            }
            else if(mcu_data.state==State::RUNE_BIG && this_x_axis<selected_x_axis)/*右旋，选左*/
            {
                selected_index=i;
                selected_x_axis=this_x_axis;
            }
        }
    }

    if(selected_index==-1){
        selected_rect.width=0;
        selected_rect.height=0;
    }
    else{
        selected_rect=box[selected_index];
        selected_rect_id=6;
    }
}

cv::Rect SolvePos::OutpostPredict(const cv::Rect &selected_rect){
    /*如果是左旋，则往左给定值预测；如果是右旋，则往右给定值预测*/
    static cv::Rect last_rect = cv::Rect(0,0,0,0);
    cv::Rect predict_rect=selected_rect;
    static int avg_width;
    if(last_rect.height == 0 || abs(selected_rect.tl().x- last_rect.tl().x) > 2*selected_rect.width)
    {
        last_rect = selected_rect;
        avg_width = last_rect.width;
        return predict_rect;
    }

    if(mcu_data.state==State::RUNE_SMALL){
        avg_width = avg_width*0.8+0.2*selected_rect.width;
        predict_rect.x = (predict_rect.x-2*avg_width > 0)?(predict_rect.x-2*avg_width):0;
    }
    else{
        avg_width = avg_width*0.8+0.2*selected_rect.width;
        int max_width = cameraParam.resolution_width - selected_rect.width;
        predict_rect.x = (predict_rect.x +2*avg_width < max_width)?(predict_rect.x +2*avg_width):max_width;
    }
#ifdef DEBUG
    cv::rectangle(show_final_armor,predict_rect,cv::Scalar(0,255,0),2,16);
    cv::imshow("outpost_predict",show_final_armor);
#endif
    last_rect = selected_rect;
    return predict_rect;
}



#ifdef CNN
float SolvePos::CalcDistance(const cv::Mat &src,const cv::Rect &final_armor){
    
    cv::Rect tmp_armor;
    /*放大*/
    tmp_armor=final_armor;
    
    // tmp_armor.x=tmp_armor.x-0.2*tmp_armor.width;
    // tmp_armor.width*=1.4;



    if(tmp_armor.x<0 || tmp_armor.y<0 || tmp_armor.x+tmp_armor.width>src.cols || tmp_armor.y+tmp_armor.height>src.rows) return -1;
    cv::Mat armor = src(tmp_armor);

    
    

#ifdef DEBUG
    cv::imshow("final_armor_roi",armor);
#endif

    cv::Mat binary_color_image;

    cv::cvtColor(armor,binary_color_image,cv::COLOR_BGR2GRAY);  //light

    float thresh_color = (mcu_data.color == EnemyColor::BLUE) ? armorParam.brightness_thresh_blue : armorParam.brightness_thresh_red;
    cv::threshold(binary_color_image, binary_color_image, thresh_color, 255, cv::THRESH_BINARY);

    cv::dilate(binary_color_image, binary_color_image, cv::Mat());  /*膨胀*/

#ifdef DEBUG
    cv::imshow("bin_armor",binary_color_image);
#endif

    std::vector<std::vector<cv::Point>> contours_color; /*颜色轮廓*/
    cv::findContours(binary_color_image, contours_color, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);

    if(contours_color.empty()) return -1;
    
    std::vector<cv::RotatedRect> lights;
    for(int i=0;i<contours_color.size();i++){
        cv::RotatedRect single_light=cv::minAreaRect(contours_color[i]);

        /*垂直矩形的高度大于宽度，且面积大于最小值*/
        cv::Rect bounding_rect = single_light.boundingRect();/*得到一个垂直的矩形*/
        if (bounding_rect.height < bounding_rect.width || single_light.size.area() < armorParam.light_min_area)    
            continue;

        /*判断灯条长宽比例*/
        float height=std::max(single_light.size.height,single_light.size.width);
        float width=std::min(single_light.size.height,single_light.size.width);
        float ratio=height/width;
        if(ratio>armorParam.light_max_ratio) continue;

         /*判断灯条旋转角度*/
        float rotated_angle=(single_light.size.width>single_light.size.height?90-single_light.angle:single_light.angle);
        if(rotated_angle>armorParam.light_max_rotated_angle) continue;
        

        lights.emplace_back(single_light);
    }

    if(lights.empty()) return -1;

    float avg_height;

    std::sort(lights.begin(),lights.end(),[](const cv::RotatedRect &r1, const cv::RotatedRect &r2){return r1.center.x<r2.center.x;});

    avg_height=(std::max(lights.front().size.height,lights.front().size.width)+std::max(lights.back().size.height,lights.back().size.width))/2.0;

    avg_height=avg_height*(1+2*armorParam.shift_scale);

    static float half_camera_width = cameraParam.resolution_width / 2;
    static float half_camera_height = cameraParam.resolution_height / 2;
    static float camera_fx = cameraParam.fx;
    static float camera_fy = cameraParam.fy;

    float distance=camera_fx * armorParam.armor_height / avg_height / 1000;

    return distance;


}
#endif
void KalmanFilter::Init()
{
    this->F = (cv::Mat_<float>(6,6)<<
    1,0,0,1,0,0,
    0,1,0,0,1,0,
    0,0,1,0,0,1,
    0,0,0,1,0,0,
    0,0,0,0,1,0,
    0,0,0,0,0,1);

    this->B = (cv::Mat_<float>(6,3)<<
    0.5,0,0,
    0,0.5,0,
    0,0,0.5,
    1, 0, 0,
    0, 1, 0,
    0, 0, 1);

    this->U_cov = (cv::Mat_<float>(3,3) << 
    5,0,0,
    0,5,0,
    0,0,20);

    this->P = (cv::Mat_<float>(6,6) <<
    1,0,0,0,0,0,
    0,1,0,0,0,0,
    0,0,4,0,0,0,
    0,0,0,5,0,0,
    0,0,0,0,5,0,
    0,0,0,0,0,20
    );

    this->H = (cv::Mat_<float>(3,6)<<
    1,0,0,0,0,0,
    0,1,0,0,0,0,
    0,0,1,0,0,0);

    this->R = (cv::Mat_<float>(3,3)<<
    1.5,0,0,
    0,1.5,0,
    0,0,4.5);

    init_flag = true;
}

cv::Mat KalmanFilter::Predict(cv::Mat& X_,cv::Mat& U_)
{
    if(!init_flag)
        return cv::Mat::zeros(cv::Size(6,1),cv::DataType<float>::type);

    return F*X_ + B*U_;
}


cv::Mat KalmanFilter::Update(cv::Mat& X_,cv::Mat& Z_,cv::Mat& U_)
{
    if(!init_flag)
        return cv::Mat::zeros(cv::Size(6,1),cv::DataType<float>::type);

    this->X = X_.clone();
    this->Z = Z_.clone();
    this->U = U_.clone();
//predict
    Q = B*U_cov*B.t();
    X_predict = F*X + B*U;
    P_predict = F*P*F.t() + Q;

    K = P_predict*H.t()*(H*P_predict*H.t()+R).inv(); //K 6x3
    cv::Mat X_correct = X_predict + K*(Z - H*X_predict);
    cv::Mat P_correct = P_predict - K*H*P_predict; //6x3*3x6*6x6

    this->P = P_correct;

    return X_correct;
}

cv::Point3f SolvePos::SolveTarget(const cv::Mat &src, const cv::Rect &armor,const int id)
{
// 初始化一些常量
    cv::Point3f target;		//将来返回的坐标
    static cv::Point3f last_target{0,0,0};	//上一帧的坐标
    static int last_id = 0;	//上一帧识别到的id
    static bool jump_flag = false; 	//用来处理target在同一机器人不同侧的装甲板来回跳的情况 
    bool init_flag = (id != last_id)||jump_flag;

    static float half_camera_width = cameraParam.resolution_width/2;
    static float half_camera_height = cameraParam.resolution_height/2;
    static float camera_fx = cameraParam.fx;
    static float camera_fy = cameraParam.fy;

//解算最基本的偏航和俯仰    
    cv::Point2f armor_center = cv::Point2f(armor.x+armor.width/2.0,armor.y+armor.height/2.0);
    float yaw_offset = atan2(half_camera_width - armor_center.x, camera_fx) * rad;
    float pitch_offset = atan2(half_camera_height - armor_center.y, camera_fy) * rad;

//测距 
//用一个历史队列对box的高度进行均值滤波
    static std::queue<int> height_queue;
    static int queue_cnt = 0;
    static int queue_sum = 0;
    static float avg_height = armor.height;
    if(init_flag && (!jump_flag))
    {
        queue_cnt = 0;
        while(!height_queue.empty())
        {
            height_queue.pop();
        }
        queue_sum = 0;
    }
    else if(queue_cnt < 10)
    {
        height_queue.push(armor.height);
        queue_cnt++;
        queue_sum += armor.height;
        avg_height = queue_sum /float(queue_cnt);
    }
    else
    {
        int queue_front = height_queue.front();
        height_queue.pop();
        height_queue.push(armor.height);
        queue_sum = queue_sum - queue_front + armor.height;
        avg_height = queue_sum / float(queue_cnt);
    }

//这个是最后返回的distance
    float distance = -1;
//-----------------------

    float box_height = 127;
    float distance_box = camera_fy * box_height / avg_height / 1000; //三角测量

//用SolvePnP再次解算一遍距离,pnp的距离和三角测量的距离加权在一起作为最终距离
//但各个镜头标定参数不一样，solvePnP先不用
    if(false)
    {
        float distance_pnp = CalcDistancePnP(src,armor,id,distance_box);
        float diff = abs(distance_pnp - distance_box);
        if(diff < 1)
        {
        float weight = abs(distance_box - last_target.z) > abs(distance_pnp - last_target.z) ? 0.7 : 0.3;
        distance = weight * distance_box + (1-weight) * distance_pnp;
        }  
        else
        {
            distance = distance_box;
        }
    }
    else
    {
        distance = distance_box;
    }

//这个是最后返回的target
    target = {yaw_offset,pitch_offset,distance};
//-------------------

//卡尔曼滤波


//先获得当前的状态,六维向量分别是x y z Vx Vy Vz
//速度用帧差法获取
    static cv::Mat last_velocity = (cv::Mat_<float>(3,1) << 
    0,0,0);
    cv::Mat velocity = (cv::Mat_<float>(3,1) <<
        target.x - last_target.x, target.y - last_target.y, target.z - last_target.z);

//加速度
    cv::Mat accelaration = (cv::Mat_<float>(3,1) << 
    0,0,0);
    if(init_flag)
    {
//如果没有last_target,卡尔曼滤波无法进行所以要跳过第一帧
        last_velocity = velocity;
        last_id = id;
        last_target = target;
        jump_flag = false;
        kalman.Init();	//如果出现目标跳跃，记得重置协方差矩阵
        return target;
    }
    else
    {
//处理两侧装甲板来回跳的情况
        float displacement_x = tan(abs(velocity.at<float>(0,0))/rad)*distance;
        float displacement_y = tan(abs(velocity.at<float>(0,1))/rad)*distance;
        if(displacement_x > 0.3 || displacement_y > 0.3)
        {
            jump_flag = true;
            last_target = target;
            last_id = id;
            return target;
        }
        accelaration = velocity - last_velocity;
    }
//状态向量,也就是卡尔曼滤波中的X
    cv::Mat state_vector = (cv::Mat_<float>(6,1)<< 
    last_target.x,last_target.y,last_target.z,velocity.at<float>(0,0),velocity.at<float>(1,0),velocity.at<float>(2,0));

//当前帧的坐标作为测量值
    cv::Mat measure_vector = (cv::Mat_<float>(3,1) << 
    target.x,target.y,target.z);

//进行卡尔曼滤波
    cv::Mat state_correct = kalman.Update(state_vector,measure_vector,accelaration);

//OPTIONAL: 有了卡尔曼，当然也可以预测坐标，如果调的好的话可以上
    if(false)
    {
        cv::Mat state_predict = kalman.Predict(state_correct,accelaration);
        target = cv::Point3f{state_predict.at<float>(0,0),state_predict.at<float>(1,0),state_predict.at<float>(2,0)};
    }    
    else
    {
        target = cv::Point3f{state_correct.at<float>(0,0),state_correct.at<float>(1,0),state_correct.at<float>(2,0)};
    }
  
//更新参数然后返回    
    last_velocity = velocity;
    last_id = id;
    last_target = target;
    return target;

}
